﻿using System;
using Microsoft.Xna.Framework;
using Asteroids.Entities.Components;

namespace Asteroids.Entities.EntityProcessors
{
    public class PhysicsEntityProcessor : EntityProcessor
    {
        private Vector2 _gravityAcceleration;
        private Vector2 _terminal, _negTerminal;
        private float _delta;

        /// <summary>
        /// The gravitational pull of the world, in meters per second squared
        /// </summary>
        /// <param name="gravityVector"></param>
        public PhysicsEntityProcessor(Vector2 gravityVector = default(Vector2), Vector2 terminalVelocity = default(Vector2))
        {
            _gravityAcceleration = gravityVector;
            if (terminalVelocity == default(Vector2))
                terminalVelocity = new Vector2(float.MaxValue, float.MaxValue);
            _terminal = terminalVelocity;
            _negTerminal = -terminalVelocity;
        }

        private Vector2 _gravityAccelerationAdjusted;

        public override void Update(float delta)
        {
            _delta = delta;

            Vector2.Multiply(ref _gravityAcceleration, _delta, out _gravityAccelerationAdjusted);

            base.Update(delta);
        }

        public override void Process(Entity e)
        {
            PhysicsComponent physicsComponent = e.QueryComponent<PhysicsComponent>();
            TransformComponent transform = e.QueryComponent<TransformComponent>();

            if (physicsComponent != null && transform != null)
            {
                var accelerationVector = physicsComponent.Acceleration;
                var velocityVector = physicsComponent.Velocity;

                Vector2.Multiply(ref accelerationVector, _delta, out accelerationVector);
                Vector2.Add(ref accelerationVector, ref velocityVector, out velocityVector);
                physicsComponent.Velocity = velocityVector;

                Vector2.Add(ref _gravityAccelerationAdjusted, ref accelerationVector, out accelerationVector);
                physicsComponent.Acceleration = accelerationVector;

                var location = transform.Location;
                Vector2.Clamp(ref velocityVector, ref _negTerminal, ref _terminal, out velocityVector);
                Vector2.Add(ref location, ref velocityVector, out location);

                float angularAccel = physicsComponent.RotationalAcceleration * _delta;
                physicsComponent.RotationalVelocity += angularAccel;

                transform.Location = location;
                transform.Rotation = transform.Rotation + physicsComponent.RotationalVelocity;
            }
        }
    }
}
